/*
 * matching_test.cc
 *
 *  Created on: Oct 21, 2011
 *      Author: tqlong
 */

// Standard
#include <iostream>
#include <algorithm>

// Boost
#include <boost/program_options.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/foreach.hpp>

// OpenCV
#include <opencv/cv.h>
#include <opencv/highgui.h>

// My own
#include "../image_feature.h"
#include "../translation_matching.h"
#include "../affine_matching.h"

namespace po = boost::program_options;
po::options_description desc("Allowed options");
po::variables_map vm;

void process_options(int argc, char** argv);
void usage(int exitcode = 0);
void do_transform();
void do_feature();
void do_translation_match();
void do_affine_match();

int main(int argc, char** argv)
{
  std::cout << "hmm tests ..." << std::endl;

  process_options(argc, argv);

  boost::posix_time::ptime time_start(boost::posix_time::microsec_clock::local_time());

  if (vm.count("help"))
    usage();
  else
  {
    if (vm.count("transform"))
    {
      do_transform();
    }
    else if (vm.count("feature"))
    {
      do_feature();
    }
    else if (vm.count("tmatch"))
    {
      do_translation_match();
    }
    else if (vm.count("amatch"))
    {
      do_affine_match();
    }
  }

  boost::posix_time::ptime time_end(boost::posix_time::microsec_clock::local_time());
  boost::posix_time::time_duration duration(time_end - time_start);
  std::cerr << "Duration = " << duration << "\n";
}

void print_cv_mat64fc1(const cv::Mat& M)
{
  for (int i = 0; i < M.rows; i++)
  {
    for (int j = 0; j < M.cols; j++) std::cout << M.at<double>(i,j) << " ";
    std::cout << std::endl;
  }
}

void do_transform()
{
  if (!vm.count("input") || !vm.count("output"))
  {
    std::cerr << "Need input and output image file names" << std::endl;
    usage();
  }
  std::string inputFileName = vm["input"].as<std::string>();
  std::string outputFileName = vm["output"].as<std::string>();
  int tx = vm["tx"].as<int>();
  int ty = vm["ty"].as<int>();
  double angle = vm["angle"].as<double>();
  double scale = vm["scale"].as<double>();

  cv::Mat inputImage = cv::imread(inputFileName);
  cv::imshow("input", inputImage);


  cv::Mat transform = cv::getRotationMatrix2D(cv::Point2f(0,0), angle, scale);
  transform.at<double>(0,2) += tx;
  transform.at<double>(1,2) += ty;
  std::cout << "transform = " << std::endl; print_cv_mat64fc1(transform);

  cv::Mat outputImage;
  cv::warpAffine(inputImage, outputImage, transform, inputImage.size());
  cv::imshow("output", outputImage);

  cv::imwrite(outputFileName, outputImage);

  cv::waitKey();
}

void do_feature()
{
  if (!vm.count("input") || !vm.count("output"))
  {
    std::cerr << "Need input image and feature output file names" << std::endl;
    usage();
  }
  std::string inputFileName = vm["input"].as<std::string>();
  std::string outputFileName = vm["output"].as<std::string>();
  int threshold = vm["threshold"].as<int>();

  cv::Mat inputImage = cv::imread(inputFileName);
  FeatureList f_list;
  FeatureList::FAST_features(inputImage, f_list, threshold);
  f_list.save(outputFileName);

  f_list.mark(inputImage);
  cv::imshow("input", inputImage);
  cv::waitKey();
}

void do_translation_match()
{
  if (!vm.count("output") || !vm.count("left") || !vm.count("right") )
  {
    std::cerr << "Need left and right and output image file names" << std::endl;
    usage();
  }
  std::string leftFileName = vm["left"].as<std::string>();
  std::string rightFileName = vm["right"].as<std::string>();
  std::string outputFileName = vm["output"].as<std::string>();
  int threshold = vm["threshold"].as<int>();

  cv::Mat leftImage = cv::imread(leftFileName);
  cv::Mat rightImage = cv::imread(rightFileName);

  FeatureList lList, rList;
  FeatureList::FAST_features(leftImage, lList, threshold);
  FeatureList::FAST_features(rightImage, rList, threshold);
  NaiveTranslationMatching m;
  m.tol_ = vm["tol"].as<double>();
  m.match(lList, rList);


  lList.mark(leftImage);
  rList.mark(rightImage, cv::Scalar(255,0,255));

  // combine two image in 1 image with left and right images side by side
  // using 2 translation tranform
  cv::Mat l_r, tmp;
  cv::Size new_size(leftImage.cols+rightImage.cols, std::max(leftImage.rows,rightImage.rows));
  cv::Mat lTransform = (cv::Mat_<double>(2,3) << 1, 0, 0, 0, 1, 0);
  cv::warpAffine(leftImage, tmp, lTransform, new_size);
  cv::Mat rTransform = (cv::Mat_<double>(2,3) << 1, 0, leftImage.cols, 0, 1, 0);
  cv::warpAffine(rightImage, l_r, rTransform, new_size);
  l_r += tmp;

  m.mark(l_r, lTransform, rTransform);

  cv::imshow("left and right", l_r);
  cv::waitKey();
}

void do_affine_match()
{
  if (!vm.count("output") || !vm.count("left") || !vm.count("right") )
  {
    std::cerr << "Need left and right and output image file names" << std::endl;
    usage();
  }
  std::string leftFileName = vm["left"].as<std::string>();
  std::string rightFileName = vm["right"].as<std::string>();
  std::string outputFileName = vm["output"].as<std::string>();
  int threshold = vm["threshold"].as<int>();

  cv::Mat leftImage = cv::imread(leftFileName);
  cv::Mat rightImage = cv::imread(rightFileName);

  FeatureList lList, rList;
  FeatureList::FAST_features(leftImage, lList, threshold);
  FeatureList::FAST_features(rightImage, rList, threshold);
  NaiveAffineMatching m;
  m.tol_ = vm["tol"].as<double>();
  m.match(lList, rList);


  lList.mark(leftImage);
  rList.mark(rightImage, cv::Scalar(255,0,255));

  // combine two image in 1 image with left and right images side by side
  // using 2 translation tranform
  cv::Mat l_r, tmp;
  cv::Size new_size(leftImage.cols+rightImage.cols, std::max(leftImage.rows,rightImage.rows));
  cv::Mat lTransform = (cv::Mat_<double>(2,3) << 1, 0, 0, 0, 1, 0);
  cv::warpAffine(leftImage, tmp, lTransform, new_size);
  cv::Mat rTransform = (cv::Mat_<double>(2,3) << 1, 0, leftImage.cols, 0, 1, 0);
  cv::warpAffine(rightImage, l_r, rTransform, new_size);
  l_r += tmp;

  m.mark(l_r, lTransform, rTransform);

  std::cout << "transform = " << std::endl; print_cv_mat64fc1(m.max_transform_);

  cv::imshow("left and right", l_r);

  cv::Mat left_t;
  cv::warpAffine(leftImage, left_t, m.max_transform_, leftImage.size());
  cv::imshow("output", left_t);
  cv::waitKey();
}

void usage(int exitcode)
{
  std::cerr << desc << std::endl;
  exit(exitcode);
}

void process_options(int argc, char** argv)
{
  desc.add_options()
    ("help", "produce this usage")
    ("input", po::value<std::string>(), "input image file")
    ("output", po::value<std::string>(), "output file")
    ("left", po::value<std::string>(), "left image file")
    ("right", po::value<std::string>(), "right image file")
    ("transform", "transform an image")
    ("feature", "find image features")
    ("tmatch", "translation matching")
    ("amatch", "affine transformation matching")
    ("tx", po::value<int>()->default_value(50), "translation in x")
    ("ty", po::value<int>()->default_value(100), "translation in y")
    ("angle", po::value<double>()->default_value(0), "rotation angle")
    ("scale", po::value<double>()->default_value(1.0), "scale")
    ("threshold", po::value<int>()->default_value(40), "threshold")
    ("tol", po::value<double>()->default_value(10), "distance tolerance")
    ;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);
}

